#define SOLAR_BUILD_SHADOW_BLOCK_COLLISION
#include <algorithm/analytical_model/algorithm/cuda/GeometryUtils.cuh>
#include <algorithm/analytical_model/algorithm/cuda/Math.cuh>
#include <algorithm/analytical_model/algorithm/cuda/ShadowBlockCollision.cuh>
#include <algorithm/analytical_model/algorithm/cuda/Utils.cuh>
#include <algorithm/analytical_model/algorithm/cuda/VectorMath.cuh>
#include <algorithm/analytical_model/algorithm/cuda/grid/RectangleGrid.cuh>
#include <utils/BitUtils.h>

using namespace solar::cuda;

__host__ __device__ auto solar::cuda::intersect(const float3& origin, const float3& dir,
                                                const float3* d_heliostat_vertices,
                                                const int* d_grid_heliostat_match, int start_id,
                                                int end_id, int subheliostat_id,
                                                int number_of_sub_heliostat) -> bool
{
    bool has_intersect = false;
// #pragma unroll 4
    for (int i = start_id; i < end_id; ++i)
    {
        int sub_heliostat_index = d_grid_heliostat_match[i];
        // 不与大定日镜本身的子定日镜判断
        if (sub_heliostat_index < subheliostat_id ||
            sub_heliostat_index >= subheliostat_id + number_of_sub_heliostat)
        {
            auto [t, u, v, intersects] = rayParallelogramIntersect(
                origin, dir, d_heliostat_vertices[3 * sub_heliostat_index + 1],
                d_heliostat_vertices[3 * sub_heliostat_index],
                d_heliostat_vertices[3 * sub_heliostat_index + 2]);
            has_intersect = has_intersect || intersects;
        }
    }
    return has_intersect;
}

__host__ __device__ auto solar::cuda::collision(const float3& origin, const float3& dir,
                                                RectangleGrid* rect_grid_ptr,
                                                const float3* d_subheliostat_vertices,
                                                int helio_id) -> bool
{
    auto* rect_grid = rect_grid_ptr;
    /**
     *Step 1 - Initialization
     **/
    //  Step 1.1 Initial current position of origin in the scene.
    int3 pos = make_int3((origin - rect_grid->pos_) / rect_grid->interval_);

    //  Step 1.2 StepX, StepY, StepZ
    int3 step;
    step.x = bit::conditionalAdd2(dir.x >= 0, -1, 2);
    step.y = bit::conditionalAdd2(dir.y >= 0, -1, 2);
    step.z = bit::conditionalAdd2(dir.z >= 0, -1, 2);

    //  Step 1.3 Initial tmaxX, tmaxY, tmaxZ
    float3 t_max; // avoid divide 0
    // tMax = |pos ± interval - (origin-rectGrid.getPosition())| / dir
    t_max.x = absDivide(calTMax(dir.x, rect_grid->interval_.x, pos.x, origin.x - rect_grid->pos_.x),
                        dir.x);
    t_max.y = absDivide(calTMax(dir.y, rect_grid->interval_.y, pos.y, origin.y - rect_grid->pos_.y),
                        dir.y);
    t_max.z = absDivide(calTMax(dir.z, rect_grid->interval_.z, pos.z, origin.z - rect_grid->pos_.z),
                        dir.z);

    //  Step 1.4 Initial tDeltaX, tDeltaY, tDeltaZ
    float3 t_delta;
    t_delta.x = absDivide(rect_grid->interval_.x, dir.x);
    t_delta.y = absDivide(rect_grid->interval_.y, dir.y);
    t_delta.z = absDivide(rect_grid->interval_.z, dir.z);

    /**
     * Step 2 - Intersection
     **/
    int3 grid_index = pos;
    int grid_address = 0;

    while (grid_index.x >= 0 && grid_index.x < rect_grid->subgrid_num_.x && grid_index.y >= 0 &&
           grid_index.y < rect_grid->subgrid_num_.y && grid_index.z >= 0 &&
           grid_index.z < rect_grid->subgrid_num_.z)
    {

        grid_address = unrollIndex(grid_index, rect_grid->subgrid_num_);
        if (intersect(origin, dir, d_subheliostat_vertices, rect_grid->d_grid_helio_match_,
                      rect_grid->d_grid_helio_index_[grid_address],
                      rect_grid->d_grid_helio_index_[grid_address + 1], helio_id, 1))
        {
            return true;
        }

        // next cell location
        bool x_min_flag = false;
        bool y_min_flag = false;
        bool z_min_flag = false;

        if (less(t_max.x, t_max.y))
        {
            // x < y

            bool flag1 = lessEqual(t_max.x, t_max.z);
            bit::conditionalAdd(flag1, grid_index.x, step.x);
            x_min_flag = flag1;
            bool flag2 = lessEqual(t_max.z, t_max.x);
            bit::conditionalAdd(flag2, grid_index.z, step.z);
            z_min_flag = flag2;

        }
        else if (less(t_max.y, t_max.x))
        {
            // y < x

            bool flag1 = lessEqual(t_max.y, t_max.z);
            bit::conditionalAdd(flag1, grid_index.y, step.y);
            y_min_flag = flag1;
            bool flag2 = lessEqual(t_max.z, t_max.y);
            bit::conditionalAdd(flag2, grid_index.z, step.z);
            z_min_flag = flag2;
        }
        else
        {
            // x == y

            bool flag1 = lessEqual(t_max.x, t_max.z);
            bit::conditionalAdd(flag1, grid_index.x, step.x);
            bit::conditionalAdd(flag1, grid_index.y, step.y);
            x_min_flag = flag1;
            y_min_flag = flag1;
            bool flag2 = lessEqual(t_max.z, t_max.x);
            bit::conditionalAdd(flag2, grid_index.z, step.z);
            z_min_flag = flag2;

        }
        bit::conditionalAdd(x_min_flag, t_max.x, t_delta.x);
        bit::conditionalAdd(y_min_flag, t_max.y, t_delta.y);
        bit::conditionalAdd(z_min_flag, t_max.z, t_delta.z);
    }

    return false;
}
